#!/usr/bin/python3
# -*- coding: utf-8 -*-

import time
import B_DQ4 as B
import EventLog as log

P = B.PoseFlow()
defauleJDD = {11: 0, 12: 0, 13: 20, 14: 40, 15: 20, 16: 0
              , 21: 0, 22: 0, 23: 20, 24: 40, 25: 20, 26: 0
              , 31: 10, 32: 10, 33: 0, 34: 20, 35: 0, 36: 0, 37: 0
              , 41: 10, 42: 10, 43: 0, 44: 20, 45: 0, 46: 0, 47: 0
              , 71: 0, 72: 0}

def bigstep():
   
    JDD = {13:-40,14:60,15:-10,16:0}
    P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=40)
    
    JDD = {13:20,14:40,15:20,16:0}
    P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=30)
    for i in range(2):
        JDD = {13:50,14:0,15:20,16:0}
        P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=30)
        
        JDD = {13:50,14:40,15:0,16:0}
        P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=30)
        
        
def hand():
    for i in range(5):
        JDD = {31:20,32:i*7,33:30,34:i*10
            ,41:20,42:i*7,43:30,44:i*10}
        P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=20)
    
    for i in range(5):
        JDD = {31:20,32:5,33:-40,34:60
            ,41:20,42:5,43:-50,44:80}
        P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=15)
        JDD = {31:20,32:5,33:-40,34:80
            ,41:20,42:5,43:-50,44:60}
        P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=15)
    
    
    JDD = {31:20,32:5,33:-40,34:60
            ,41:20,42:85,43:70,44:70}
    P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=55)
    time.sleep(2)
    JDD = {41:20,42:5,43:-40,44:60
            ,31:20,32:85,33:70,34:70}
    P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=55)
    time.sleep(2)
    JDD = {41:20,42:85,43:70,44:70
            ,31:20,32:85,33:70,34:70}
    P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=55)
    
    # time.sleep(2)
    # JDD = {41:20,42:5,43:-30,44:60
    #         ,31:80,32:10,33:0,34:20}
    # P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=55)
    
    # JDD = {41:20,42:5,43:-30,44:60
    #         ,31:80,32:40,33:0,34:20}
    # P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=55)
    
    
    time.sleep(5)
    P.SetPoseFlowDic(defauleJDD, sleeptime=0.05, steps=20)

def start():
    P.SetPoseFlowDic(defauleJDD, sleeptime=0.05, steps=20)
    time.sleep(2)
 
def BlancePose(g=0, h1=20, h2=20, b1=0, b2=0, sp=10,der = [0,0],hand = [0,0]):
    # 重心向前偏移值
    q = 3
    
    # 腰部运动，根据迈步来进行调节
    if b1>8:
        waist = int(b1/1.5)
    elif b2>8:
        waist = -int(b2/1.5)
    else:
        waist = 0
    handmid = 0
    
    # JDDh1 = {13: h1 + b1 - q, 14: h1 * 2, 15: h1 - b1 + q}
    # JDDh2 = {23: h2 + b2 - q, 24: h2 * 2, 25: h2 - b2 + q}
    JDDh1 = {13: h1 + b1 - q, 14: h1 * 2, 15: h1 - b1}
    JDDh2 = {23: h2 + b2 - q, 24: h2 * 2, 25: h2 - b2}
    JDDg = {12: g+der[0], 16: -g, 22: -g+der[1], 26: g}
    JDD = {}
    JDD.update(JDDh1)
    JDD.update(JDDh2)
    JDD.update(JDDg)
    
    # JDDh = {32:2,42:2,34:40,44:40,31:hand[0]-handmid,41:hand[1]-handmid,72:waist,21:-waist,11:waist}
    # print(hand[0],hand[1],hand[0]+15,hand[1]+15)
    JDDh = {31:0,41:0,32:10,42:10,34:int(hand[0]*0.5+15),44:int(hand[1]*0.5+15),31:hand[0],41:hand[1],71:waist,21:-waist,11:waist,72:0}
    JDD.update(JDDh)
    
    # log.logger.info(JDD)
    P.SetPoseFlowDic(JDD, sleeptime=0.05, steps=sp)
    
def LegPose(g=0, h1=20, h2=20, b1=0, b2=0, sp=10,der = [0,0],hand = [0,0],cut = {13:[-2,2]}):
    # 重心向前偏移值
    q = 4
    
    # 腰部运动，根据迈步来进行调节
    if b1>8:
        waist = int(b1/1.5)
    elif b2>8:
        waist = -int(b2/1.5)
    else:
        waist = 0
    handmid = 0
    
    JDDh1 = {13: h1 + b1 - q, 14: h1 * 2, 15: h1 - b1}
    JDDh2 = {23: h2 + b2 - q, 24: h2 * 2, 25: h2 - b2}
    JDDg = {12: g+der[0], 16: -g, 22: -g+der[1], 26: g}
    JDD = {}
    JDD.update(JDDh1)
    JDD.update(JDDh2)
    JDD.update(JDDg)
    
    # JDDh = {32:2,42:2,34:40,44:40,31:hand[0]-handmid,41:hand[1]-handmid,72:waist,21:-waist,11:waist}
    print(hand[0],hand[1],hand[0]+15,hand[1]+15)
    JDDh = {34:int(hand[0]*0.5+15),44:int(hand[1]*0.5+15),31:hand[0],41:hand[1],71:waist,21:-waist,11:waist,72:0}
    JDD.update(JDDh)
    
    log.logger.info(JDD)
    P.SetPoseFlowDic(JDD,JDDcut=cut, sleeptime=0.05, steps=sp)



 
if __name__ == "__main__":
    
    
    start()
    
    hand()
    
    start()
    
    bigstep()
    
    stepi = 1
    log.logger.info("The %sth setp : Start walk pose" % stepi)
    BlancePose(g=0, h1=20, h2=20, b1=0, b2=0, sp=30)  # 起步
    time.sleep(2)
    
    
    
    log.logger.info("start-leg-L2")
    BlancePose(g=8, h1=20, h2=20, b1=0, b2=0, sp=10)  # 缓冲
    LegPose(g=9.5, h1=35, h2=20, b1=0, b2=0, sp=15,der = [0,0],hand=[-10, 10],cut = {13:[-2,0]})  # 抬腿
    LegPose(g=8, h1=20, h2=20, b1=10, b2=-2, sp=15,der = [0,0],hand=[-20, 20],cut = {13:[0,2]})  # 落腿

    for i in range(5):
        # BlancePose(g=0, h1=20, h2=20, b1=8, b2=-8, sp=10)  # 变换
        log.logger.info("Leg L2")
        BlancePose(g=-7, h1=20, h2=17, b1=1, b2=-10, sp=12)  # 缓冲
        LegPose(g=-8.5, h1=20, h2=30, b1=0, b2=-6, sp=7,der = [0,0],hand=[10, -10],cut = {23:[-2,0]})  # 抬腿
        LegPose(g=-7, h1=20, h2=21, b1=-2, b2=10, sp=5,der = [0,0],hand=[20, -20],cut = {23:[0,2]})  # 落腿
        
        # BlancePose(g=0, h1=20, h2=20, b1=-8, b2=8, sp=20)  # 变换
        log.logger.info("Leg L1")
        BlancePose(g=7.5, h1=17, h2=20, b1=-10, b2=2, sp=12)  # 缓冲
        LegPose(g=8.5, h1=30, h2=20, b1=-6, b2=0, sp=7,der = [0,0],hand=[-10, 10],cut = {13:[-2,0]})  # 抬腿
        LegPose(g=7, h1=20, h2=20, b1=10, b2=-2, sp=7,der = [0,0],hand=[-20, 20],cut = {13:[0,2]})  # 落腿
    
    BlancePose(g=0, h1=20, h2=20, b1=0, b2=0, sp=30)  # 起步